package v2;
public class RK4Integrator2D {

	private static State2D evaluate(Acceleratable acc, State2D initial, double dt, State2D d) {
		State2D state = new State2D();
		state.setPX(initial.getPX() + d.getPX() * dt);
		state.setPY(initial.getPY() + d.getPY() * dt);
		state.setVX(initial.getVX() + d.getVX() * dt);
		state.setVY(initial.getVY() + d.getVY() * dt);

		State2D output = new State2D();
		output.setPX(state.getVX());
		output.setPY(state.getVY());
		output.setV(acc.getAcc().toDoubleArr());
		return output;
	}

	public static void integrate(Acceleratable acc, double dt) {
		State2D state = (State2D) acc.getState();
		
		State2D a = evaluate(acc, state, 0.0f, new State2D());
		State2D b = evaluate(acc, state, dt * 0.5f, a);
		State2D c = evaluate(acc, state, dt * 0.5f, b);
		State2D d = evaluate(acc, state, dt, c);

		double dpxdt = 1.0f / 6.0f * (a.getPX() + 2.0f
				* (b.getPX() + c.getPX()) + d.getPX());
		double dpydt = 1.0f / 6.0f * (a.getPY() + 2.0f
				* (b.getPY() + c.getPY()) + d.getPY());
		double dvxdt = 1.0f / 6.0f * (a.getVX() + 2.0f
				* (b.getVX() + c.getVX()) + d.getVX());
		double dvydt = 1.0f / 6.0f * (a.getVY() + 2.0f
				* (b.getVY() + c.getVY()) + d.getVY());

		state.setPX(state.getPX() + dpxdt * dt);
		state.setPY(state.getPY() + dpydt * dt);
		state.setVX(state.getVX() + dvxdt * dt);
		state.setVY(state.getVY() + dvydt * dt);
	}
}
